//$CC test_domain.c -o ectest_domain -L /home/lee/igh-mc -lethercat
//scp ectest_domain root@192.168.58.1:/home/mc
#include "config_domain.h"

void check_master_state(void)
{
    ec_master_state_t ms;

    ecrt_master_state(master, &ms);

    if (ms.slaves_responding != master_state.slaves_responding) {
        printf("found %u slave(s).\n", ms.slaves_responding);
    }
    if (ms.al_states != master_state.al_states) {
        printf("AL states: 0x%02X.\n", ms.al_states);
    }
    if (ms.link_up != master_state.link_up) {
        printf("Link is %s.\n", ms.link_up ? "up" : "down");
    }

    master_state = ms;
}

/*****************************************************************************/

void check_slave0_config_states(void)
{
    ec_slave_config_state_t s;

    ecrt_slave_config_state(sc_cooldrive0, &s);

    if (s.al_state != sc_cooldrive0_state.al_state) {
        printf("CoolDrive0: State 0x%02X.\n", s.al_state);
    }
    if (s.online != sc_cooldrive0_state.online) {
        printf("CoolDrive0: %s.\n", s.online ? "online" : "offline");
    }
    if (s.operational != sc_cooldrive0_state.operational) {
        printf("CoolDrive0: %soperational.\n", s.operational ? "" : "Not ");
    }

    sc_cooldrive0_state = s;
}
/*****************************************************************************/

void check_slave1_config_states(void)
{
    ec_slave_config_state_t s;

    ecrt_slave_config_state(sc_cooldrive1, &s);

    if (s.al_state != sc_cooldrive1_state.al_state) {
        printf("CoolDrive1: State 0x%02X.\n", s.al_state);
    }
    if (s.online != sc_cooldrive1_state.online) {
        printf("CoolDrive1: %s.\n", s.online ? "online" : "offline");
    }
    if (s.operational != sc_cooldrive1_state.operational) {
        printf("CoolDrive1: %soperational.\n", s.operational ? "" : "Not ");
    }

    sc_cooldrive1_state = s;
}
/*****************************************************************************/

void check_domainInput_state(void)
{
    ec_domain_state_t ds;

    ecrt_domain_state(domainServoInput, &ds);

    if (ds.working_counter != domainServoInput_state.working_counter) {
        printf("Domain1: WC %u.\n", ds.working_counter);
    }
    if (ds.wc_state != domainServoInput_state.wc_state) {
        printf("Domain1: State %u.\n", ds.wc_state);
    }

    domainServoInput_state = ds;
}
/*****************************************************************************/

void check_domainOutput_state(void)
{
    ec_domain_state_t ds;

    ecrt_domain_state(domainServoOutput, &ds);

    if (ds.working_counter != domainServoOutput_state.working_counter) {
        printf("Domain1: WC %u.\n", ds.working_counter);
    }
    if (ds.wc_state != domainServoOutput_state.wc_state) {
        printf("Domain1: State %u.\n", ds.wc_state);
    }

    domainServoOutput_state = ds;
}
/*****************************************************************************/


int main(void)
{
    
    master = ecrt_request_master(0);
    if (!master) {
        return -1;
    }
    printf("step1:request_master sucess\n");
    check_master_state();
    
    printf("step2:master get slave config\n");
    sc_cooldrive0=ecrt_master_slave_config(master,CoolDrivePos0,CoolDrive); 
    sc_cooldrive1=ecrt_master_slave_config(master,CoolDrivePos1,CoolDrive);       
    //check_slave_config_states();
    printf("step2.1:master_reset,Retry configuring slaves.\n");
    ecrt_master_reset(master); 
    printf("step2.2:check salve config states.\n");
    check_slave0_config_states();
    check_slave1_config_states();
    
    
    //pause();
    printf("step3:create domain.\n");
    domainServoOutput = ecrt_master_create_domain(master);
    domainServoInput = ecrt_master_create_domain(master);
    printf("step3.1:domain reg pdo entry.\n");
    /*if (ecrt_domain_reg_pdo_entry_list(domainServoOutput, domainServoOutput_regs)) {
        fprintf(stderr, "PDO entry registration failed!\n");
        return -1;
    }
    if (ecrt_domain_reg_pdo_entry_list(domainServoInput, domainServoInput_regs)) {
        fprintf(stderr, "PDO entry registration failed!\n");
        return -1;
    }*/
    ecrt_slave_config_reg_pdo_entry(sc_cooldrive0,0x6040, 0x00,domainServoInput,&A1.cntlwd);
    ecrt_slave_config_reg_pdo_entry(sc_cooldrive0,0x6041, 0x00,domainServoOutput,&A1.status);
    printf("step3.2:check domain.\n");
    check_domainInput_state();
    check_domainOutput_state();
    printf("step4:configure SYNC signals.\n");
    // configure SYNC signals for this slave
    ecrt_slave_config_dc(sc_cooldrive0, 0x0300, 1000000, 0, 0, 0);
    
    printf("xenomai Activating master...\n");
    if (ecrt_master_activate(master)) {
        printf("xenomai Activating master...failed\n");
        return -1;
    }
    /********************/
    if (!(domainInput_pd = ecrt_domain_data(domainServoInput))) {
        fprintf(stderr, "xenomai Failed to get domain data pointer.\n");
        return -1;
    }
    if (!(domainOutput_pd = ecrt_domain_data(domainServoOutput))) {
        fprintf(stderr, "xenomai Failed to get domain data pointer.\n");
        return -1;
    }
    printf("xenomai Activating master...success\n");
    
    ecrt_master_receive(master);
    ecrt_domain_process(domainServoOutput);
    ecrt_domain_process(domainServoInput);
    check_domainInput_state();
    check_domainOutput_state();
    
    pause();
    printf("release_master\n");
    ecrt_release_master(master);
    
    return 0;
}



